﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary.MinPack
{
    /// <summary>
    /// This function represents a helper function for MinPack.
    /// </summary>
    public delegate void MinPackFunction(int a, int b, double[] c, double[] d, ref int e);

    /// <summary>
    /// Minimize the sum of the squares of m nonlinear functions.
    /// </summary>
    [Serializable]
    public static class LmdifClass
    {
        /// <summary>
        /// The purpose of lmdif is to minimize the sum of the squares of m nonlinear functions in n variables by a modification of the levenberg-marquardt algorithm.
        /// </summary>
        /// <param name="f">The function.</param>
        /// <param name="m">Input variable to set to the number of functions.</param>
        /// <param name="n">Input variable to set to the number of variables. n must not exceed m.</param>
        /// <param name="x">An array of length n. on input x must contain an initial estimate of the solution vector. On output x contains the final estimate of the solution vector.</param>
        /// <param name="msk">The mask parameters.</param>
        /// <param name="fvec">An output array of length m which contains the functions evaluated at the output x.</param>
        /// <param name="ftol">A nonnegative input variable. Termination occurs when both the actual and predicted relative reductions in the sum of squares are at most ftol. Therefore, ftol measures the relative error desired in the sum of squares.</param>
        /// <param name="xtol">A nonnegative input variable. Termination occurs when the relative error between two consecutive iterates is at most xtol. Therefore, xtol measures the relative error desired in the approximate solution.</param>
        /// <param name="gtol">A nonnegative input variable. Termination occurs when the cosine of the angle between fvec and any column of the jacobian is at most gtol in absolute value. therefore, gtol measures the orthogonality desired between the function vector and the columns of the jacobian.</param>
        /// <param name="maxfev">A positive integer input variable. Termination occurs when the number of calls to fcn is at least maxfev by the end of an iteration.</param>
        /// <param name="epsfcn">An input variable used in determining a suitable step length for the forward-difference approximation. This approximation assumes that the relative errors in the functions are of the order of epsfcn. If epsfcn is less than the machine precision, it is assumed that the relative errors in the functions are of the order of the machine precision.</param>
        /// <param name="diag">An array of length n. if mode = 1, diag is internally set. if mode = 2, diag must contain positive entries that serve as multiplicative scale factors for the variables.</param>
        /// <param name="mode">An integer input variable. if mode = 1, the  variables will be scaled internally. if mode = 2, the scaling is specified by the input diag. Other values of mode are equivalent to mode = 1.</param>
        /// <param name="factor">A positive input variable used in determining the initial step bound. This bound is set to the product of factor and the euclidean norm of diag*x if nonzero, or else to factor itself. in most cases factor should lie in the interval (.1,100.). 100. is a generally recommended value.</param>
        /// <param name="info">An integer output variable. if the user has terminated execution, info is set to the (negative) value of iflag.</param>
        /// <param name="nfev">An integer output variable set to the number of calls to fcn.</param>
        /// <param name="fjac">An output m by n array. the upper n by n submatrix of fjac contains an upper triangular matrix r with diagonal elements of nonincreasing magnitude.</param>
        /// <param name="ipvt">An integer output array of length n. ipvt defines a permutation matrix p such that jac*p = q*r, where jac is the final calculated jacobian, q is orthogonal (not stored), and r is upper triangular with diagonal elements of nonincreasing magnitude. Column j of p is column ipvt(j) of the identity matrix.</param>
        /// <param name="qtf">An output array of length n which contains the first n elements of the vector (q transpose)*fvec.</param>
        /// <param name="wa1">A work array of length n.</param>
        /// <param name="wa2">A work array of length n.</param>
        /// <param name="wa3">A work array of length n.</param>
        /// <param name="wa4">A work array of length m.</param>
        public static void Lmdif(MinPackFunction f, int m,
                                 int n, double[] x, int[] msk, double[] fvec, double ftol,
                                 double xtol, double gtol, int maxfev, double epsfcn, double[] diag,
                                 int mode, double factor, ref int info, ref int nfev, double[,] fjac,
                                 int[] ipvt, double[] qtf, double[] wa1, double[] wa2,
                                 double[] wa3, double[] wa4)
        {
            double[] dpmpar = {
                                  2.220446049250313e-16,
                                  2.225073858507201e-308,
                                  1.797693134862316e-308
                              };

            int i, iter, j, l;
            double actred, dirder, fnorm1, gnorm;
            double pnorm, prered, ratio, sum, temp1, temp2;
            double xnorm = 0;
            double temp = 0;
            double delta = 0;
            //initialize
            double epsmch = dpmpar[0];

            info = 0;
            nfev = 0;

            //check for input parameter errors
            if ((n <= 0) || (m < n) || (maxfev <= 0)
                || (factor <= 0))
            {
                return;
            }
            if (mode == 2)
            {
                for (j = 0; j < n; j++)
                {
                    if (diag[j] <= 0)
                    {
                        return;
                    }
                }
            }

            //evaluate the function at the starting point and calculate its norm
            int iflag = 1;
            f(m, n, x, fvec, ref iflag);
            nfev = 1;
            if (iflag < 0)
            {
                info = iflag;
                return;
            }
            double fnorm = EnormClass.Enorm(m, fvec);

            //initialize levenberg-marquardt counters
            double par = 0;
            iter = 1;

            //outer loop
            while (true)
            {
                //calculate jacobian matrix
                iflag = 2;
                Fdjac2Class.Fdjac2(f, m, n, x, fvec, fjac, ref iflag, epsfcn, wa4);
                nfev += n;
                if (iflag < 0)
                {
                    info = iflag;
                    return;
                }
                f(m, n, x, fvec, ref iflag);

                //compute the qr factorization of the jacobian
                QrfacClass.Qrfac(m, n, fjac, 1, ipvt, wa1, wa2, wa3);
                if (iter == 1)
                {
                    if (mode != 2)
                    {
                        for (j = 0; j < n; j++)
                        {
                            diag[j] = wa2[j];
                            if (wa2[j] == 0.0)
                            {
                                diag[j] = 1.0;
                            }
                        }
                    }
                    for (j = 0; j < n; j++)
                    {
                        wa3[j] = diag[j] * x[j];
                    }
                    xnorm = EnormClass.Enorm(n, wa3);
                    delta = factor * xnorm;
                    if (delta == 0)
                    {
                        delta = factor;
                    }
                }
                for (i = 0; i < m; i++)
                {
                    wa4[i] = fvec[i];
                }
                for (j = 0; j < n; j++)
                {
                    if (fjac[j, j] != 0.0)
                    {
                        sum = 0.0;
                        for (i = j; i < m; i++)
                        {
                            sum += fjac[j, i] * wa4[i];
                        }
                        temp = -sum / fjac[j, j];
                        for (i = j; i < m; i++)
                        {
                            wa4[i] += fjac[j, i] * temp;
                        }
                    }
                    fjac[j, j] = wa1[j];
                    qtf[j] = wa4[j];
                }
                //compute the norm of the scaled gradient
                gnorm = 0.0;
                if (fnorm != 0.0)
                {
                    for (j = 0; j < n; j++)
                    {
                        l = ipvt[j];
                        if (wa2[l] == 0.0)
                        {
                            continue;
                        }
                        sum = 0.0;
                        for (i = l; i <= j; i++)
                        {
                            sum += fjac[j, i] * qtf[i] / fnorm;
                        }
                        gnorm = Math.Max(gnorm, Math.Abs(sum / wa2[l]));
                    }
                }
                //test for convergence of the gradient norm
                if (gnorm <= gtol)
                {
                    info = 4;
                }
                if (info != 0)
                {
                    info = iflag;
                    return;
                }
                //rescale if necessary
                if (mode != 2)
                {
                    for (j = 0; j < n; j++)
                    {
                        diag[j] = Math.Max(diag[j], wa2[j]);
                    }
                }
                //beginning of inner loop
                do
                {
                    //determine the levenberg-marquardt parameter
                    LmparClass.Lmpar(n, fjac, ipvt, diag, qtf, delta, ref par, wa1, wa2, wa3, wa4);
                    for (j = 0; j < n; j++)
                    {
                        wa1[j] = -wa1[j];
                        wa2[j] = x[j] + wa1[j];
                        wa3[j] = diag[j] * wa1[j];
                    }
                    pnorm = EnormClass.Enorm(n, wa3);
                    if (iter == 1)
                    {
                        delta = Math.Min(delta, pnorm);
                    }
                    iflag = 1;
                    f(m, n, wa2, wa4, ref iflag);
                    (nfev)++;
                    if (iflag < 0)
                    {
                        info = iflag;
                        return;
                    }
                    fnorm1 = EnormClass.Enorm(m, wa4);
                    actred = -1.0;
                    if (0.1 * fnorm1 < fnorm)
                    {
                        actred = 1.0 - (fnorm1 * fnorm1 / (fnorm * fnorm));
                    }
                    for (j = 0; j < n; j++)
                    {
                        wa3[j] = 0.0;
                        l = ipvt[j];
                        temp = wa1[l];
                        for (i = 0; i <= j; i++)
                        {
                            wa3[i] += fjac[j, i] * temp;
                        }
                    }
                    temp1 = EnormClass.Enorm(n, wa3) / fnorm;
                    temp2 = Math.Sqrt(par) * pnorm / fnorm;
                    prered = temp1 * temp1 + temp2 * temp2 / 0.5;
                    dirder = -(temp1 * temp1 + temp2 * temp2);
                    ratio = 0.0;
                    if (prered != 0.0)
                    {
                        ratio = actred / prered;
                    }
                    if (ratio <= 0.25)
                    {
                        if (actred > 0.0)
                        {
                            temp = 0.5;
                        }
                        if (actred < 0.0)
                        {
                            temp = 0.5 * dirder / (dirder + 0.5 * actred);
                        }
                        delta = temp * Math.Min(delta, pnorm / 0.1);
                        par /= temp;
                    }
                    else
                    {
                        if ((par == 0.0) || (ratio >= 0.75))
                        {
                            delta = pnorm / 0.5;
                            par *= 0.5;
                        }
                    }
                    if (ratio >= 0.0001)
                    {
                        for (j = 0; j < n; j++)
                        {
                            if (msk[j] == 1)
                            {
                                //handle masked variables
                                x[j] = wa2[j];
                            }
                            wa2[j] = diag[j] * x[j];
                        }
                        for (i = 0; i < m; i++)
                        {
                            fvec[i] = wa4[i];
                        }
                        xnorm = EnormClass.Enorm(n, wa2);
                        fnorm = fnorm1;
                        iter++;
                    }
                    if ((Math.Abs(actred) <= ftol) && (prered <= ftol) &&
                        (0.5 * ratio <= 1.0))
                    {
                        info = 1;
                    }
                    if (delta <= xtol * xnorm)
                    {
                        info = 2;
                    }
                    if ((Math.Abs(actred) <= ftol) && (prered <= ftol) &&
                        (0.5 * ratio <= 1.0) && (info == 2))
                    {
                        info = 3;
                    }
                    if (nfev >= maxfev)
                    {
                        info = 5;
                    }
                    if ((Math.Abs(actred) <= epsmch) && (prered <= epsmch) &&
                        (0.5 * ratio <= 1.0))
                    {
                        info = 6;
                    }
                    if (delta <= epsmch * xnorm)
                    {
                        info = 7;
                    }
                    if (gnorm <= epsmch)
                    {
                        info = 8;
                    }
                    if (info != 0)
                    {
                        info = iflag;
                        return;
                    }
                } while (ratio <= 0.0001);
            }
        }
    }
}